#include <iostream>
#include <string.h>
#include <vector>
#include <stdexcept>
#include <math.h>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "stewart.h"

using namespace std;
using namespace Eigen;


stewart::stewart(double DownR,double UpR,double down_alpha, double up_beta):_DownR(DownR),_UpR(UpR),_down_alpha(down_alpha),_up_beta(up_beta) 
{   
    _initPose.resize(6);
    _konwn_length.resize(6);
    _inv_result.resize(6);
}

stewart::~stewart()
{
}

/**
 * 
*/
bool stewart::up_down_P_calcu(Eigen::Vector3d (& pDown)[6],  Eigen::Vector3d (& pUp)[6],double down_R,double up_R,double down_alpha,double up_beta)
{
    for(int i=0;i<6;i++)
    {
        //down platform
        int sign = i%2==0?-1:1;
        int times = i/2;
        pDown[i][0]= down_R*cos(-M_PI/2+sign*down_alpha/2+2*times*M_PI/3);
        pDown[i][1]= down_R*sin (-M_PI/2+sign*down_alpha/2+2*times*M_PI/3);
        pDown[i][2] = 0;

        pUp[i][0] = up_R*cos(-M_PI/2+sign*up_beta/2+2*times*M_PI/3);
        pUp[i][1] = up_R*sin (-M_PI/2+sign*up_beta/2+2*times*M_PI/3);
        pUp[i][2] =0;// initHeight;
    } 
   return true ;
};

Eigen::Matrix<double,6,6>  stewart::jaco_calcu2(    Eigen::Vector3d _pDown[6],  Eigen::Vector3d _pUp[6],Eigen::VectorXd _pPose)     
{
double b1_x,b1_y,b1_z,b2_x,b2_y,b2_z,b3_x,b3_y,b3_z,b4_x,b4_y,b4_z,b5_x,b5_y,b5_z,b6_x,b6_y,b6_z;
double P1_x,P1_y,P1_z,P2_x,P2_y,P2_z,P3_x,P3_y,P3_z,P4_x,P4_y,P4_z,P5_x,P5_y,P5_z,P6_x,P6_y,P6_z;
double x1,x2,x3,x4,x5,x6;
b1_x = _pDown[0][0];b1_y = _pDown[0][1];b1_z = _pDown[0][2];
b2_x = _pDown[1][0];b2_y = _pDown[1][1];b2_z = _pDown[1][2];
b3_x = _pDown[2][0];b3_y = _pDown[2][1];b3_z = _pDown[2][2];
b4_x = _pDown[3][0];b4_y = _pDown[3][1];b4_z = _pDown[3][2];
b5_x = _pDown[4][0];b5_y = _pDown[4][1];b5_z = _pDown[4][2];
b6_x = _pDown[5][0];b6_y = _pDown[5][1];b6_z = _pDown[5][2];

P1_x = _pUp[0][0];P1_y = _pUp[0][1];P1_z = _pUp[0][2];
P2_x = _pUp[1][0];P2_y = _pUp[1][1];P2_z = _pUp[1][2];
P3_x = _pUp[2][0];P3_y = _pUp[2][1];P3_z = _pUp[2][2];
P4_x = _pUp[3][0];P4_y = _pUp[3][1];P4_z = _pUp[3][2];
P5_x = _pUp[4][0];P5_y = _pUp[4][1];P5_z = _pUp[4][2];
P6_x = _pUp[5][0];P6_y = _pUp[5][1];P6_z = _pUp[5][2];

x1 = _pPose[0];          x2 = _pPose[1];          x3 = _pPose[2];
Eigen::Quaterniond qua;//声明一个Eigen类的四元数
// qua.x() = _pPose(3);qua.y() = _pPose(4);qua.z() = _pPose(5);qua.w() = _pPose(6);

//四元数转为旋转矩阵--先归一化再转为旋转矩阵
Eigen::Matrix3d desR;
Eigen::Vector3d desV;
desR= qua.normalized().toRotationMatrix();  
desV=desR.eulerAngles(2,1,0);

// x4 = desV[0];  x5 = desV[1];  x6 = desV[2];
x4 =  _pPose[3];  x5 =  _pPose[4];  x6 =  _pPose[5];

Eigen::Matrix<double,6,6>  jaco;
jaco<<
2*x1 - 2*b1_x - 2*P1_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P1_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P1_x*cos(x5)*cos(x6), 2*x2 - 2*b1_y + 2*P1_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P1_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P1_x*cos(x5)*sin(x6), 2*x3 - 2*b1_z - 2*P1_x*sin(x5) + 2*P1_z*cos(x4)*cos(x5) + 2*P1_y*cos(x5)*sin(x4), 2*(P1_y*cos(x4)*cos(x5) - P1_z*cos(x5)*sin(x4))*(x3 - b1_z - P1_x*sin(x5) + P1_z*cos(x4)*cos(x5) + P1_y*cos(x5)*sin(x4)) + 2*(P1_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P1_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b1_x - P1_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P1_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P1_x*cos(x5)*cos(x6)) - 2*(P1_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P1_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b1_y + P1_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P1_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P1_x*cos(x5)*sin(x6)), 2*cos(x6)*(P1_z*cos(x4)*cos(x5) - P1_x*sin(x5) + P1_y*cos(x5)*sin(x4))*(x1 - b1_x + P1_x*cos(x5)*cos(x6) - P1_y*cos(x4)*sin(x6) + P1_z*sin(x4)*sin(x6) + P1_z*cos(x4)*cos(x6)*sin(x5) + P1_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P1_x*cos(x5) + P1_z*cos(x4)*sin(x5) + P1_y*sin(x4)*sin(x5))*(x3 - b1_z - P1_x*sin(x5) + P1_z*cos(x4)*cos(x5) + P1_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P1_z*cos(x4)*cos(x5) - P1_x*sin(x5) + P1_y*cos(x5)*sin(x4))*(x2 - b1_y + P1_y*cos(x4)*cos(x6) + P1_x*cos(x5)*sin(x6) - P1_z*cos(x6)*sin(x4) + P1_z*cos(x4)*sin(x5)*sin(x6) + P1_y*sin(x4)*sin(x5)*sin(x6)), 2*(P1_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P1_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P1_x*cos(x5)*cos(x6))*(x2 - b1_y + P1_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P1_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P1_x*cos(x5)*sin(x6)) - 2*(P1_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P1_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P1_x*cos(x5)*sin(x6))*(x1 - b1_x - P1_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P1_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P1_x*cos(x5)*cos(x6)),
2*x1 - 2*b2_x - 2*P2_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P2_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P2_x*cos(x5)*cos(x6), 2*x2 - 2*b2_y + 2*P2_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P2_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P2_x*cos(x5)*sin(x6), 2*x3 - 2*b2_z - 2*P2_x*sin(x5) + 2*P2_z*cos(x4)*cos(x5) + 2*P2_y*cos(x5)*sin(x4), 2*(P2_y*cos(x4)*cos(x5) - P2_z*cos(x5)*sin(x4))*(x3 - b2_z - P2_x*sin(x5) + P2_z*cos(x4)*cos(x5) + P2_y*cos(x5)*sin(x4)) + 2*(P2_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P2_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b2_x - P2_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P2_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P2_x*cos(x5)*cos(x6)) - 2*(P2_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P2_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b2_y + P2_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P2_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P2_x*cos(x5)*sin(x6)), 2*cos(x6)*(P2_z*cos(x4)*cos(x5) - P2_x*sin(x5) + P2_y*cos(x5)*sin(x4))*(x1 - b2_x + P2_x*cos(x5)*cos(x6) - P2_y*cos(x4)*sin(x6) + P2_z*sin(x4)*sin(x6) + P2_z*cos(x4)*cos(x6)*sin(x5) + P2_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P2_x*cos(x5) + P2_z*cos(x4)*sin(x5) + P2_y*sin(x4)*sin(x5))*(x3 - b2_z - P2_x*sin(x5) + P2_z*cos(x4)*cos(x5) + P2_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P2_z*cos(x4)*cos(x5) - P2_x*sin(x5) + P2_y*cos(x5)*sin(x4))*(x2 - b2_y + P2_y*cos(x4)*cos(x6) + P2_x*cos(x5)*sin(x6) - P2_z*cos(x6)*sin(x4) + P2_z*cos(x4)*sin(x5)*sin(x6) + P2_y*sin(x4)*sin(x5)*sin(x6)), 2*(P2_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P2_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P2_x*cos(x5)*cos(x6))*(x2 - b2_y + P2_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P2_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P2_x*cos(x5)*sin(x6)) - 2*(P2_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P2_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P2_x*cos(x5)*sin(x6))*(x1 - b2_x - P2_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P2_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P2_x*cos(x5)*cos(x6)),
2*x1 - 2*b3_x - 2*P3_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P3_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P3_x*cos(x5)*cos(x6), 2*x2 - 2*b3_y + 2*P3_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P3_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P3_x*cos(x5)*sin(x6), 2*x3 - 2*b3_z - 2*P3_x*sin(x5) + 2*P3_z*cos(x4)*cos(x5) + 2*P3_y*cos(x5)*sin(x4), 2*(P3_y*cos(x4)*cos(x5) - P3_z*cos(x5)*sin(x4))*(x3 - b3_z - P3_x*sin(x5) + P3_z*cos(x4)*cos(x5) + P3_y*cos(x5)*sin(x4)) + 2*(P3_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P3_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b3_x - P3_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P3_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P3_x*cos(x5)*cos(x6)) - 2*(P3_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P3_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b3_y + P3_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P3_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P3_x*cos(x5)*sin(x6)), 2*cos(x6)*(P3_z*cos(x4)*cos(x5) - P3_x*sin(x5) + P3_y*cos(x5)*sin(x4))*(x1 - b3_x + P3_x*cos(x5)*cos(x6) - P3_y*cos(x4)*sin(x6) + P3_z*sin(x4)*sin(x6) + P3_z*cos(x4)*cos(x6)*sin(x5) + P3_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P3_x*cos(x5) + P3_z*cos(x4)*sin(x5) + P3_y*sin(x4)*sin(x5))*(x3 - b3_z - P3_x*sin(x5) + P3_z*cos(x4)*cos(x5) + P3_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P3_z*cos(x4)*cos(x5) - P3_x*sin(x5) + P3_y*cos(x5)*sin(x4))*(x2 - b3_y + P3_y*cos(x4)*cos(x6) + P3_x*cos(x5)*sin(x6) - P3_z*cos(x6)*sin(x4) + P3_z*cos(x4)*sin(x5)*sin(x6) + P3_y*sin(x4)*sin(x5)*sin(x6)), 2*(P3_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P3_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P3_x*cos(x5)*cos(x6))*(x2 - b3_y + P3_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P3_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P3_x*cos(x5)*sin(x6)) - 2*(P3_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P3_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P3_x*cos(x5)*sin(x6))*(x1 - b3_x - P3_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P3_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P3_x*cos(x5)*cos(x6)),
2*x1 - 2*b4_x - 2*P4_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P4_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P4_x*cos(x5)*cos(x6), 2*x2 - 2*b4_y + 2*P4_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P4_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P4_x*cos(x5)*sin(x6), 2*x3 - 2*b4_z - 2*P4_x*sin(x5) + 2*P4_z*cos(x4)*cos(x5) + 2*P4_y*cos(x5)*sin(x4), 2*(P4_y*cos(x4)*cos(x5) - P4_z*cos(x5)*sin(x4))*(x3 - b4_z - P4_x*sin(x5) + P4_z*cos(x4)*cos(x5) + P4_y*cos(x5)*sin(x4)) + 2*(P4_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P4_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b4_x - P4_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P4_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P4_x*cos(x5)*cos(x6)) - 2*(P4_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P4_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b4_y + P4_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P4_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P4_x*cos(x5)*sin(x6)), 2*cos(x6)*(P4_z*cos(x4)*cos(x5) - P4_x*sin(x5) + P4_y*cos(x5)*sin(x4))*(x1 - b4_x + P4_x*cos(x5)*cos(x6) - P4_y*cos(x4)*sin(x6) + P4_z*sin(x4)*sin(x6) + P4_z*cos(x4)*cos(x6)*sin(x5) + P4_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P4_x*cos(x5) + P4_z*cos(x4)*sin(x5) + P4_y*sin(x4)*sin(x5))*(x3 - b4_z - P4_x*sin(x5) + P4_z*cos(x4)*cos(x5) + P4_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P4_z*cos(x4)*cos(x5) - P4_x*sin(x5) + P4_y*cos(x5)*sin(x4))*(x2 - b4_y + P4_y*cos(x4)*cos(x6) + P4_x*cos(x5)*sin(x6) - P4_z*cos(x6)*sin(x4) + P4_z*cos(x4)*sin(x5)*sin(x6) + P4_y*sin(x4)*sin(x5)*sin(x6)), 2*(P4_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P4_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P4_x*cos(x5)*cos(x6))*(x2 - b4_y + P4_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P4_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P4_x*cos(x5)*sin(x6)) - 2*(P4_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P4_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P4_x*cos(x5)*sin(x6))*(x1 - b4_x - P4_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P4_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P4_x*cos(x5)*cos(x6)),
2*x1 - 2*b5_x - 2*P5_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P5_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P5_x*cos(x5)*cos(x6), 2*x2 - 2*b5_y + 2*P5_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P5_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P5_x*cos(x5)*sin(x6), 2*x3 - 2*b5_z - 2*P5_x*sin(x5) + 2*P5_z*cos(x4)*cos(x5) + 2*P5_y*cos(x5)*sin(x4), 2*(P5_y*cos(x4)*cos(x5) - P5_z*cos(x5)*sin(x4))*(x3 - b5_z - P5_x*sin(x5) + P5_z*cos(x4)*cos(x5) + P5_y*cos(x5)*sin(x4)) + 2*(P5_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P5_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b5_x - P5_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P5_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P5_x*cos(x5)*cos(x6)) - 2*(P5_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P5_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b5_y + P5_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P5_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P5_x*cos(x5)*sin(x6)), 2*cos(x6)*(P5_z*cos(x4)*cos(x5) - P5_x*sin(x5) + P5_y*cos(x5)*sin(x4))*(x1 - b5_x + P5_x*cos(x5)*cos(x6) - P5_y*cos(x4)*sin(x6) + P5_z*sin(x4)*sin(x6) + P5_z*cos(x4)*cos(x6)*sin(x5) + P5_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P5_x*cos(x5) + P5_z*cos(x4)*sin(x5) + P5_y*sin(x4)*sin(x5))*(x3 - b5_z - P5_x*sin(x5) + P5_z*cos(x4)*cos(x5) + P5_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P5_z*cos(x4)*cos(x5) - P5_x*sin(x5) + P5_y*cos(x5)*sin(x4))*(x2 - b5_y + P5_y*cos(x4)*cos(x6) + P5_x*cos(x5)*sin(x6) - P5_z*cos(x6)*sin(x4) + P5_z*cos(x4)*sin(x5)*sin(x6) + P5_y*sin(x4)*sin(x5)*sin(x6)), 2*(P5_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P5_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P5_x*cos(x5)*cos(x6))*(x2 - b5_y + P5_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P5_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P5_x*cos(x5)*sin(x6)) - 2*(P5_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P5_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P5_x*cos(x5)*sin(x6))*(x1 - b5_x - P5_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P5_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P5_x*cos(x5)*cos(x6)),
2*x1 - 2*b6_x - 2*P6_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + 2*P6_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + 2*P6_x*cos(x5)*cos(x6), 2*x2 - 2*b6_y + 2*P6_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - 2*P6_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + 2*P6_x*cos(x5)*sin(x6), 2*x3 - 2*b6_z - 2*P6_x*sin(x5) + 2*P6_z*cos(x4)*cos(x5) + 2*P6_y*cos(x5)*sin(x4), 2*(P6_y*cos(x4)*cos(x5) - P6_z*cos(x5)*sin(x4))*(x3 - b6_z - P6_x*sin(x5) + P6_z*cos(x4)*cos(x5) + P6_y*cos(x5)*sin(x4)) + 2*(P6_y*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P6_z*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)))*(x1 - b6_x - P6_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P6_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P6_x*cos(x5)*cos(x6)) - 2*(P6_y*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P6_z*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)))*(x2 - b6_y + P6_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P6_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P6_x*cos(x5)*sin(x6)), 2*cos(x6)*(P6_z*cos(x4)*cos(x5) - P6_x*sin(x5) + P6_y*cos(x5)*sin(x4))*(x1 - b6_x + P6_x*cos(x5)*cos(x6) - P6_y*cos(x4)*sin(x6) + P6_z*sin(x4)*sin(x6) + P6_z*cos(x4)*cos(x6)*sin(x5) + P6_y*cos(x6)*sin(x4)*sin(x5)) - 2*(P6_x*cos(x5) + P6_z*cos(x4)*sin(x5) + P6_y*sin(x4)*sin(x5))*(x3 - b6_z - P6_x*sin(x5) + P6_z*cos(x4)*cos(x5) + P6_y*cos(x5)*sin(x4)) + 2*sin(x6)*(P6_z*cos(x4)*cos(x5) - P6_x*sin(x5) + P6_y*cos(x5)*sin(x4))*(x2 - b6_y + P6_y*cos(x4)*cos(x6) + P6_x*cos(x5)*sin(x6) - P6_z*cos(x6)*sin(x4) + P6_z*cos(x4)*sin(x5)*sin(x6) + P6_y*sin(x4)*sin(x5)*sin(x6)), 2*(P6_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) - P6_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P6_x*cos(x5)*cos(x6))*(x2 - b6_y + P6_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P6_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P6_x*cos(x5)*sin(x6)) - 2*(P6_y*(cos(x4)*cos(x6) + sin(x4)*sin(x5)*sin(x6)) - P6_z*(cos(x6)*sin(x4) - cos(x4)*sin(x5)*sin(x6)) + P6_x*cos(x5)*sin(x6))*(x1 - b6_x - P6_y*(cos(x4)*sin(x6) - cos(x6)*sin(x4)*sin(x5)) + P6_z*(sin(x4)*sin(x6) + cos(x4)*cos(x6)*sin(x5)) + P6_x*cos(x5)*cos(x6));
return jaco;
};

Eigen::VectorXd  stewart::inverse_fun2(Eigen::VectorXd pose,Eigen::VectorXd length, Eigen::Vector3d pDown[6],  Eigen::Vector3d pUp[6])
{
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix= Eigen::AngleAxisd(pose[3], Eigen::Vector3d::UnitX()) *
                                    Eigen::AngleAxisd(pose[4], Eigen::Vector3d::UnitY()) *
                                    Eigen::AngleAxisd(pose[5], Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d desR;
    // desR = desT.block<3,3>(0,0);
    desR = rotation_matrix;

        Eigen::VectorXd lAxis(6) ,total_lenth(6); 
        Eigen::Vector3d tempP,uptempP;
        uptempP(0) =  pose(0);uptempP(1) = pose(1);uptempP(2) = pose(2);
        for(int m=0;m<6;m++)
        {
            // cout<<uptempP<<endl;
            tempP=desR*pUp[m] +uptempP;
            // cout<<tempP<<endl;
           lAxis(m)= sqrt(pow(tempP[0]-pDown[m][0],2)+pow(tempP[1]-pDown[m][1],2)+pow(tempP[2]-pDown[m][2],2)) -length(m);
           total_lenth(m)= sqrt(pow(tempP[0]-pDown[m][0],2)+pow(tempP[1]-pDown[m][1],2)+pow(tempP[2]-pDown[m][2],2));
            
            // cout<<"Delta total_lenth "<<m<<"="<<lAxis[m]<<endl;
            // cout<<"Axis total_lenth "<<m<<"="<<total_lenth[m]<<endl;
        }
    return lAxis;
};


